// --------------------------------------------------------------------------------------------------------------------
// <copyright file="MoveAndGraspFSM.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------

namespace Microsoft.Robotics.Manipulation.PicknPlace
{
    using System;
    using System.Collections.Generic;
    using System.Runtime.Serialization;
    using Microsoft.Robotics.FiniteStateMachine;
    using Microsoft.Robotics.Numerics;
    using Microsoft.Robotics.Runtime;

    /// <summary>
    /// Executes a state machine for a collection of move and grasp sub tasks
    /// </summary>
    public class MoveAndGraspFSM : Machine<PicknPlaceFSMContext, PicknPlaceFSMContext, PicknPlaceFSMContext>
    {
        /// <summary>
        /// Time out used to resend arm move message in mSec
        /// </summary>
        private int armMoveRetryTimeOut = 3500;

        /// <summary>
        /// Initializes a new instance of the <see cref="MoveAndGraspFSM" /> class.
        /// </summary>
        /// <param name="postArmMove">Delegate for arm moves</param>
        /// <param name="postHandMove">Delegate for hand moves</param>
        /// <param name="subTasks">List of move and grasp task to execute</param>
        /// <param name="context">Context data for the state machine</param>
        public MoveAndGraspFSM(
            Action<Pose> postArmMove, 
            Action<GripperStatus> postHandMove, 
            MoveAndGraspTaskDescription[] subTasks,
            PicknPlaceFSMContext context)
        {
            int iterCount = 0;
            this.Context = context;
            DateTime armMoveTimer = DateTime.Now;

            State<PicknPlaceFSMContext> start = new State<PicknPlaceFSMContext>(
                "Start state",
                c =>
                {
                    iterCount = 0;
                    return c;
                });

            State<PicknPlaceFSMContext> actionState = new State<PicknPlaceFSMContext>(
            "Action state",
             c =>
             {
                 if (iterCount < subTasks.Length)
                 {
                     c.PreviousArmTargetPose = (c.TargetTask != null) ? c.TargetTask.ArmPose : c.CurrentArmPose;
                     c.TargetTask = subTasks[iterCount];
                     c.StartMovePose = c.CurrentArmPose;
                     postHandMove(c.TargetTask.GripperState);
                     postArmMove(c.TargetTask.ArmPose);
                     ++iterCount;
                     c.StartSubTaskTimer(out armMoveTimer);
                 }

                 return c;
             });

            start.AddTransition(t => { return true; }, actionState);

            actionState.AddTransition(
                t =>
                {
                    if (iterCount < subTasks.Length)
                    {
                        bool ret = t.IsArmMoveComplete() && t.IsHandReady();

                        if (false == ret)
                        {
                            t.StartSubTaskTimer();
                        }

                        return ret && t.IsSubTaskTimeOut(t.TargetTask.DelayInMsec);
                    }

                    return false;
                },
                 actionState);

            // Resend the arm move message if the arm did not move 
            // from its initial pose after the resend time out
            // This work around should be revisited after control messages are moved to TCP (task 2025)
            actionState.AddTransition(
            t =>
            {
                bool ret = false;

                if (iterCount < subTasks.Length)
                {
                    ret = ((t.IsArmMoveComplete(t.StartMovePose) || t.IsArmMoveComplete(t.PreviousArmTargetPose)) && t.IsSubTaskTimeOut(this.armMoveRetryTimeOut, armMoveTimer));
                    if (true == ret && iterCount > 0)
                    {
                        --iterCount;
                    }
                }

                return ret;
            },
             actionState);

            this.Initialize(this.Context, start);
        }

        /// <summary>
        /// Step the FSM
        /// </summary>
        /// <param name="context">FSM context</param>
        public void Step(PicknPlaceFSMContext context)
        {
            this.Update(this.Context);
        }

        /// <summary>
        /// String representation for debugging and testing.
        /// </summary>
        /// <returns>String representation.</returns>
        public override string ToString()
        {
            return this.Context.TargetTask.Description;
        }
    }
}
